%% MOD-DH参数
%定义连杆的D-H参数
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
%建立机器人模型
%       theta  d        a        alpha     
L1=Link([0     d1       a1       alpha1     ],'modified');
L2=Link([0     d2       a2       alpha2     ],'modified');L2.offset = pi/2;
L3=Link([0     d3       a3       alpha3     ],'modified');
L4=Link([0     d4       a4       alpha4     ],'modified');
L5=Link([0     d5       a5       alpha5     ],'modified');
L6=Link([0     d6       a6       alpha6     ],'modified');
%限制机器人的关节空间
L1.qlim = [(-165/180)*pi,(165/180)*pi];
L2.qlim = [(-95/180)*pi, (70/180)*pi];
L3.qlim = [(-85/180)*pi, (95/180)*pi];
L4.qlim = [(-180/180)*pi,(180/180)*pi];
L5.qlim = [(-115/180)*pi,(115/180)*pi];
L6.qlim = [(-360/180)*pi,(360/180)*pi];
%连接连杆，机器人取名为myrobot
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','myrobot');
robot.plot([0,0,0,0,0,0]);%输出机器人模型，后面的六个角为输出时的theta姿态
robot.display();%打印出机器人D-H参数表
robot.teach;%展示机器人模型
hold on;

%% 机器人模型定义
% 使用提供的机器人建模代码定义机器人模型

%% 定义障碍物
obstacle = [500, 200, 500]; % 障碍物中心坐标 [x, y, z]
obstacle_radius = 100; % 障碍物半径

%% 路径规划
start_config = [0, 0, 0, 0, 0, 0]; % 起始关节角度
end_config = [pi/2, pi/4, -pi/4, pi/2, -pi/4, pi/3]; % 目标关节角度

num_points = 50; % 路径点数量
path = zeros(num_points, 6);

for i = 1:num_points
    t = (i - 1) / (num_points - 1);
    path(i, :) = start_config + t * (end_config - start_config);
end

%% 避障检查和路径调整
max_iterations = 100;
for iter = 1:max_iterations
    collision_found = false;
    
    for i = 1:num_points
        % 计算当前构型下的末端执行器位置
        T = robot.fkine(path(i, :));
        pos = T.t';
        
        % 检查是否与障碍物碰撞
        if norm(pos - obstacle) < obstacle_radius
            collision_found = true;
            
            % 计算避障方向
            avoid_dir = pos - obstacle;
            avoid_dir = avoid_dir / norm(avoid_dir);
            
            % 调整路径点
            adjustment = 10 * avoid_dir; % 调整步长
            new_pos = pos + adjustment;
            
            % 使用逆运动学计算新的关节角度
            new_config = robot.ikine(SE3(new_pos), 'mask', [1 1 1 0 0 0]);
            path(i, :) = new_config;
        end
    end
    
    if ~collision_found
        break;
    end
end

%% 可视化结果
figure;
robot.plot(start_config);
hold on;

% 绘制障碍物
[X, Y, Z] = sphere(20);
X = X * obstacle_radius + obstacle(1);
Y = Y * obstacle_radius + obstacle(2);
Z = Z * obstacle_radius + obstacle(3);
surf(X, Y, Z, 'FaceColor', 'r', 'FaceAlpha', 0.3, 'EdgeColor', 'none');

% 绘制路径
for i = 1:num_points
    T = robot.fkine(path(i, :));
    pos = T.t';
    plot3(pos(1), pos(2), pos(3), 'b.', 'MarkerSize', 10);
end

% 绘制起点和终点
T_start = robot.fkine(start_config);
T_end = robot.fkine(end_config);
plot3(T_start.t(1), T_start.t(2), T_start.t(3), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot3(T_end.t(1), T_end.t(2), T_end.t(3), 'ro', 'MarkerSize', 10, 'LineWidth', 2);

title('机械臂避障路径规划');
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');
grid on;
view(3);

%% 动画展示
figure;
robot.plot(path);
